import time
import smbus

'''
mpu6050
i2c 两根线
gnd+5V 两根线
i2cdetec -y -1  查看设备地址

'''

# 定义mpu6050 internal reg addr
DEVICE_ADDR =   0x68

PWR_MGMT_1  =   0x6b
SMPLRT_DIV  =   0x19
CONFIG      =   0x1a
CYRO_CONFIG =   0x1b
INT_ENABLE  =   0x38
ACCEL_X_H   =   0x3b
ACCEL_Y_H   =   0x3d
ACCEL_Z_H   =   0x3f

GYRO_X_H   =   0x43
GYRO_Y_H   =   0x45
GYRO_Z_H   =   0x47

bus = sbmus.SMBus(1)        #或入参传0. 老的pi 用的0

def mpu_init():
    # 抽样速率
    bus.write(DEVICE_ADDR,SMPLRT_DIV,7)
    # power manage  reg
    bus.write(DEVICE_ADDR, PWR_MGMT_1, 1)
    # config reg
    bus.write(DEVICE_ADDR, CONFIG, 0)
    # 陀螺配置
    bus.write(DEVICE_ADDR, CYRO_CONFIG, 24)
    # 中断使能
    bus.write(DEVICE_ADDR, INT_ENABLE, 1)
    print("init mpu6050 ok")

def read_raw_data(addr):
    high = bus.read_byte_data(DEVICE_ADDR,addr)
    low = bus.read_byte_data(DEVICE_ADDR, addr+1)
    value = (high<<8) | low;
    if value > 32768:       #是个负数
        value-=65535
    return value
def loop():
    mpu_init()
    while True:
        acc_x = read_raw_data(ACCEL_X_H)
        acc_y = read_raw_data(ACCEL_Y_H)
        acc_z = read_raw_data(ACCEL_Z_H)

        gyro_x = read_raw_data(GYRO_X_H)
        gyro_y = read_raw_data(GYRO_Y_H)
        gyro_z = read_raw_data(GYRO_Z_H)

        Ax=acc_x/16384.0
        Ay = acc_y / 16384.0
        Az = acc_z / 16384.0

        Gx = gyro_x / 131.0
        Gy = gyro_y / 131.0
        Gz = gyro_z / 131.0

        print("{}-{}-{}  {}-{}-{}".format(Ax,Ay,Az,Gx,Gy,Gz))
        time.sleep(0.5)


def destroy(): #释放资源
    GPIO.cleanup()

if __name__=='__main__':
    try:
        loop()
    except KeyboardInterrupt:   #按下 ctrl+c 释放资源
        destroy()